#include "Header.h"

char Message[50];
uint8_t ID;

int main(void)
{
	Servo_Init();
	OLED_Init();
	SpeedTest_Init();
	Motor_Init();
	MPU6050_Init();
	Servo_SetAngle(90);
	OLED_ShowHexNum(1, 4, ID, 2);
	hc05_Init();
	while (1)
	{
		OLED_ShowSignedNum(4, 8, Angle, 5);
		sprintf(Message, "%d %d %d %d %d %d %d %d %d %d %d %d\r\n", Encoder_Speed1, Encoder_Speed2, (int16_t)Out1, (int16_t)Out2, (int16_t)Out_Angle, (int16_t)Angle, (int16_t)Vz, (int16_t)Target_Angle, Target_Left, Target_Right, (int16_t)VL, (int16_t)VR); //(int16_t)Out1,(int16_t)Out2
		hc05_SendString(Message);
	}
}
